// ########################## DEFINES ##########################
#define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME         0xABCD     	// [-] Start frme definition for reliable serial communication
#define TIME_SEND           100         // [ms] Sending time interval
#define SPEED_MAX_TEST      500         // [-] Maximum speed for testing
// #define DEBUG_RX                        // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
#define ONOFF_PIN           4

#include <SoftwareSerial.h>
SoftwareSerial RikibotSerial(2,3);        // RX, TX

// Global variables
uint8_t idx = 0;                        // Index for new data pointer
uint16_t bufStartFrame;                 // Buffer Start Frame
byte *p;                                // Pointer declaration for the new received data
byte incomingByte;
byte incomingBytePrev;

typedef struct{
    uint16_t  start;          //0XABCD
    int16_t   rpmR;            //右轮转速
    int16_t   rpmL;           //左轮转速
    int16_t   batVoltage;     //主板电压
    int16_t   boardTemp;      //主板温度
    int16_t   curL_DC;        //左轮电流
    int16_t   curR_DC;        //右轮流
    uint16_t  checksum;       //核验
} RikibotFeedback;
RikibotFeedback Feedback;
RikibotFeedback NewFeedback;

typedef struct{
   uint16_t start;    //0xABCD
   int16_t  pwmR;     //右轮pwm
   int16_t  pwmL;     //左轮pwm
   uint16_t checksum;  //校验
} RikibotCommand;
RikibotCommand Command;
 

// ########################## SETUP ##########################
void setup() 
{
  Serial.begin(SERIAL_BAUD);
  Serial.println("Rikibot Foc Serial v1.0");

  RikibotSerial.begin(SERIAL_BAUD);

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(ONOFF_PIN, OUTPUT);
  digitalWrite(ONOFF_PIN, HIGH);
  delay(1000);
  digitalWrite(ONOFF_PIN, LOW);
}

// ########################## SEND ##########################
void Send(int16_t pwmR, int16_t pwmL)
{
  // Create command
  Command.start    = (uint16_t)START_FRAME;
  Command.pwmR    = (int16_t)pwmR;
  Command.pwmL    = (int16_t)pwmL;
  Command.checksum = (uint16_t)(Command.start ^ Command.pwmR ^ Command.pwmL);

  // Write to Serial
  RikibotSerial.write((uint8_t *) &Command, sizeof(Command)); 
}

// ########################## RECEIVE ##########################
void Receive()
{
    // Check for new data availability in the Serial buffer
    if (RikibotSerial.available()) {
        incomingByte 	  = RikibotSerial.read();                                   // Read the incoming byte
        bufStartFrame	= ((uint16_t)(incomingByte) << 8) | incomingBytePrev;       // Construct the start frame
    }
    else {
        return;
    }

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
        Serial.print(incomingByte);
        return;
    #endif

    // Copy received data
    if (bufStartFrame == START_FRAME) {	                    // Initialize if new data is detected
        p       = (byte *)&NewFeedback;
        *p++    = incomingBytePrev;
        *p++    = incomingByte;
        idx     = 2;	
    } else if (idx >= 2 && idx < sizeof(RikibotFeedback)) {  // Save the new received data
        *p++    = incomingByte; 
        idx++;
    }	
    
    // Check if we reached the end of the package
    if (idx == sizeof(RikibotFeedback)) {
        uint16_t checksum;
        checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.rpmR ^ NewFeedback.rpmL
                            ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC);

        // Check validity of the new data
        if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
            // Copy the new data
            memcpy(&Feedback, &NewFeedback, sizeof(RikibotFeedback));
            // Print data to built-in Serial
            Serial.print(" 1: ");  Serial.print(Feedback.rpmR);
            Serial.print(" 2: ");  Serial.print(Feedback.rpmL);
            Serial.print(" 3: ");  Serial.print(Feedback.batVoltage);
            Serial.print(" 4: ");  Serial.print(Feedback.boardTemp);
            Serial.print(" 5: ");  Serial.println(Feedback.curL_DC);
            Serial.print(" 6: ");  Serial.println(Feedback.curR_DC);
        } else {
          Serial.println("Non-valid data skipped");
        }
        idx = 0;    // Reset the index (it prevents to enter in this if condition in the next cycle)
    }

    // Update previous states
    incomingBytePrev = incomingByte;
}

// ########################## LOOP ##########################

unsigned long iTimeSend = 0;
int iTestMax = SPEED_MAX_TEST;
int iTest = 0;

void loop(void)
{ 
  unsigned long timeNow = millis();

  // Check for new received data
  Receive();

  // Send commands
  if (iTimeSend > timeNow) return;
  iTimeSend = timeNow + TIME_SEND;
  int pwm_speed = SPEED_MAX_TEST - 2*abs(iTest);
  Send(pwm_speed, pwm_speed);

  // Calculate test command signal
  iTest += 10;
  if (iTest > iTestMax) iTest = -iTestMax;

  // Blink the LED
  digitalWrite(LED_BUILTIN, (timeNow%2000)<1000);
}
